#pragma once

#include <iostream>
#include <math.h>
#include <cmath>
#include "standard/Vec3f.hpp"

#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif



namespace siut {
  namespace simd {
    /** \brief matrix and vector calculation tool using standard C++ language */

    namespace standard {

      /** \brief Standard: class for doing matrix 4x4 calculations */

      struct Mat3f
      {
	float v_[9];

	Mat3f()
	{

	};
	


	
#ifndef NVCC
	Mat3f(const Vec3f &x, const Vec3f &y, const Vec3f &z)
	{
	  //v_[0 ] = x.x();
	  //v_[1 ] = x.y();
	  //v_[2 ] = x.z();
	  //
	  //v_[3 ] = y.x();
	  //v_[4 ] = y.y();
	  //v_[5 ] = y.z();
	  //
	  //v_[6 ] = z.x();
	  //v_[7 ] = z.y();
	  //v_[8 ] = z.z();
		// Set columns
		v_[0 ] = x.x();
		v_[1 ] = y.x();
		v_[2 ] = z.x();
		
		v_[3 ] = x.y();
		v_[4 ] = y.y();
		v_[5 ] = z.y();
		
		v_[6 ] = x.z();
		v_[7 ] = y.z();
		v_[8 ] = z.z();
	}
	
	//by columns
	Mat3f(float m_11, float m_21, float m_31, 
		     float m_12, float m_22, float m_32,
		     float m_13, float m_23, float m_33)
	{
	  v_[0 ] = m_11;
	  v_[1 ] = m_21;
	  v_[2 ] = m_31;
	  
	  v_[3 ] = m_12;
	  v_[4 ] = m_22;
	  v_[5 ] = m_32;
	  
	  v_[6 ] = m_13;
	  v_[7 ] = m_23;
	  v_[8 ] = m_33;
 
	}
  
  
	Mat3f(const float *pf)
	{
	  v_[0 ] = pf[0 ];
	  v_[1 ] = pf[1 ];
	  v_[2 ] = pf[2 ];
	  
	  v_[3 ] = pf[0 ];
	  v_[4 ] = pf[1 ];
	  v_[5 ] = pf[2 ];
	  
	  v_[6 ] = pf[0 ];
	  v_[7 ] = pf[1 ];
	  v_[8 ] = pf[2 ];
	}
  
	Mat3f(float tab1[3] , float tab2[3] , float tab3[3])
	{
	  v_[0 ] = tab1[0 ];
	  v_[1 ] = tab1[1 ];
	  v_[2 ] = tab1[2 ];
	  
	  v_[3 ] = tab1[0 ];
	  v_[4 ] = tab2[1 ];
	  v_[5 ] = tab2[2 ];
	  
	  v_[6 ] = tab2[0 ];
	  v_[7 ] = tab2[1 ];
	  v_[8 ] = tab3[2 ];
	}
  
	Mat3f(float tab[9])
	{
	  v_[0 ] = tab[0 ];
	  v_[1 ] = tab[1 ];
	  v_[2 ] = tab[2 ];
	  v_[3 ] = tab[3 ];
	  v_[4 ] = tab[4 ];
	  v_[5 ] = tab[5 ];
	  v_[6 ] = tab[6 ];
	  v_[7 ] = tab[7 ];
	  v_[8 ] = tab[8 ];
	}
  
	Mat3f(const Mat3f &v)
	{
	  v_[0 ] = v.v_[0 ];
	  v_[1 ] = v.v_[1 ];
	  v_[2 ] = v.v_[2 ];
	  v_[3 ] = v.v_[3 ];
	  v_[4 ] = v.v_[4 ];
	  v_[5 ] = v.v_[5 ];
	  v_[6 ] = v.v_[6 ];
	  v_[7 ] = v.v_[7 ];
	  v_[8 ] = v.v_[8 ];
	}
  
	Mat3f(const float s)  
	{
	  v_[0 ] = s;
	  v_[1 ] = s;
	  v_[2 ] = s;
	  v_[3 ] = s;
	  v_[4 ] = s;
	  v_[5 ] = s;
	  v_[6 ] = s;
	  v_[7 ] = s;
	  v_[8 ] = s;

	}   
#endif

	/** Returns the ite element by columns. (obs. first element har index 0) */
	inline float & operator[](int i) 
	{ 
		return v_[i]; 
	}
	/** Returns the ite element by columns. (obs. first element har index 0) */
	inline const float & operator[](int i) const { return v_[i]; }
	
	}; // end struct Mat3f after all ctors and accessors, rest of the funcs belong in the namespace Mat3f

	

      /*-----------------------------------------------------------------------*/
      /*    operators +=  -=  *= /=                                            */
      /*-----------------------------------------------------------------------*/
 
      inline void operator += (Mat3f &m, const Mat3f &n)
      {
	m.v_[0 ] += n.v_[0 ];
	m.v_[1 ] += n.v_[1 ];
	m.v_[2 ] += n.v_[2 ];
	m.v_[3 ] += n.v_[3 ];
	m.v_[4 ] += n.v_[4 ];
	m.v_[5 ] += n.v_[5 ];
	m.v_[6 ] += n.v_[6 ];
	m.v_[7 ] += n.v_[7 ];
	m.v_[8 ] += n.v_[8 ];

      }
      inline void operator += (Mat3f &m, const float s)
      {
	m.v_[0 ] += s;
	m.v_[1 ] += s;
	m.v_[2 ] += s;
	m.v_[3 ] += s;
	m.v_[4 ] += s;
	m.v_[5 ] += s;
	m.v_[6 ] += s;
	m.v_[7 ] += s;
	m.v_[8 ] += s;

      }

      inline void operator -= (Mat3f &m, const Mat3f &n)
      {
	m.v_[0 ] -= n.v_[0 ];
	m.v_[1 ] -= n.v_[1 ];
	m.v_[2 ] -= n.v_[2 ];
	m.v_[3 ] -= n.v_[3 ];
	m.v_[4 ] -= n.v_[4 ];
	m.v_[5 ] -= n.v_[5 ];
	m.v_[6 ] -= n.v_[6 ];
	m.v_[7 ] -= n.v_[7 ];
	m.v_[8 ] -= n.v_[8 ];

      } 
	  inline void operator -= (Mat3f &m, const float s)
	  {
		  m.v_[0 ] -= s;
		  m.v_[1 ] -= s;
		  m.v_[2 ] -= s;
		  m.v_[3 ] -= s;
		  m.v_[4 ] -= s;
		  m.v_[5 ] -= s;
		  m.v_[6 ] -= s;
		  m.v_[7 ] -= s;
		  m.v_[8 ] -= s;

	  }
  
	
	inline Vec3f operator * (const Vec3f &vector, const Mat3f &m)
	{
		return Vec3f(
			vector.x() * m[0] + vector.y() * m[1] + vector.z() * m[2],
			vector.x() * m[3] + vector.y() * m[4] + vector.z() * m[5],
			vector.x() * m[6] + vector.y() * m[7] + vector.z() * m[8]
		);
	}
	

      inline Vec3f operator * (const Mat3f &m, const Vec3f &vector)
      {
		return Vec3f(
			vector.x() * m[0] + vector.y() * m[1] + vector.z() * m[2],
			vector.x() * m[3] + vector.y() * m[4] + vector.z() * m[5],
			vector.x() * m[6] + vector.y() * m[7] + vector.z() * m[8]
		);
      }
	
	/**
	* Convenience matrix-vector transformation function
	*/
	inline Vec3f transformTranspose(const Mat3f &m, const Vec3f &vector)
	{
		return Vec3f(
			vector.x() * m[0] + vector.y() * m[3] + vector.z() * m[6],
			vector.x() * m[1] + vector.y() * m[4] + vector.z() * m[7],
			vector.x() * m[2] + vector.y() * m[5] + vector.z() * m[8]
		);
	}

 /**
	 * Returns a matrix which is this matrix multiplied by the given
	 * other matrix.
	 */
	inline Mat3f operator*(const Mat3f &m, const Mat3f &n)
	{
		return Mat3f(
			m[0]*n[0] + m[1]*n[3] + m[2]*n[6],
			m[0]*n[1] + m[1]*n[4] + m[2]*n[7],
			m[0]*n[2] + m[1]*n[5] + m[2]*n[8],

			m[3]*n[0] + m[4]*n[3] + m[5]*n[6],
			m[3]*n[1] + m[4]*n[4] + m[5]*n[7],
			m[3]*n[2] + m[4]*n[5] + m[5]*n[8],

			m[6]*n[0] + m[7]*n[3] + m[8]*n[6],
			m[6]*n[1] + m[7]*n[4] + m[8]*n[7],
			m[6]*n[2] + m[7]*n[5] + m[8]*n[8]
			);
	}

	inline void operator *= (Mat3f &m, const Mat3f &n)
	{  
#ifndef NVCC
		Mat3f res(m);
#else
		Mat3f res;
		copyInto(res, m);
#endif

		m.v_[ 0] = res.v_[0]*n.v_[0] + res.v_[1]*n.v_[3] + res.v_[2]*n.v_[6] ;
		m.v_[ 1] = res.v_[0]*n.v_[1] + res.v_[1]*n.v_[4] + res.v_[2]*n.v_[7] ;
		m.v_[ 2] = res.v_[0]*n.v_[0] + res.v_[1]*n.v_[5] + res.v_[2]*n.v_[8] ;
		
		m.v_[ 3] = res.v_[3]*n.v_[0] + res.v_[4]*n.v_[3] + res.v_[5]*n.v_[6] ;
		m.v_[ 4] = res.v_[3]*n.v_[1] + res.v_[4]*n.v_[4] + res.v_[5]*n.v_[7] ;
		m.v_[ 5] = res.v_[3]*n.v_[2] + res.v_[4]*n.v_[5] + res.v_[5]*n.v_[8] ;
		
		m.v_[ 6] = res.v_[6]*n.v_[0] + res.v_[7]*n.v_[3] + res.v_[8]*n.v_[6] ; 
		m.v_[ 7] = res.v_[6]*n.v_[1] + res.v_[7]*n.v_[4] + res.v_[8]*n.v_[7] ;
		m.v_[ 8] = res.v_[6]*n.v_[2] + res.v_[7]*n.v_[5] + res.v_[8]*n.v_[8] ;


	}
	inline void operator *= (Mat3f &m, const float s)
	{
		m.v_[0 ] *= s;
		m.v_[1 ] *= s;
		m.v_[2 ] *= s;

		m.v_[3 ] *= s;
		m.v_[4 ] *= s;
		m.v_[5 ] *= s;

		m.v_[6 ] *= s;
		m.v_[7 ] *= s;
		m.v_[8 ] *= s;
	}

	/**
	 * Sets this matrix to be the rotation matrix corresponding to
	 * the given quaternion.
	 */
	 /*
	void setOrientation(const Quatf &q)
	{
		v_[0] = 1 - (2*q.j*q.j + 2*q.k*q.k);
		v_[1] = 2*q.i*q.j + 2*q.k*q.r;
		v_[2] = 2*q.i*q.k - 2*q.j*q.r;
		v_[3] = 2*q.i*q.j - 2*q.k*q.r;
		v_[4] = 1 - (2*q.i*q.i  + 2*q.k*q.k);
		v_[5] = 2*q.j*q.k + 2*q.i*q.r;
		v_[6] = 2*q.i*q.k + 2*q.j*q.r;
		v_[7] = 2*q.j*q.k - 2*q.i*q.r;
		v_[8] = 1 - (2*q.i*q.i  + 2*q.j*q.j);
	}
	*/
	
	
	inline Mat3f inverse(const Mat3f &m)                           
      {
	Mat3f res;
	
            float t4 = m[0]*m[4];
            float t6 = m[0]*m[5];
            float t8 = m[1]*m[3];
            float t10 = m[2]*m[3];
            float t12 = m[1]*m[6];
            float t14 = m[2]*m[6];

            // Calculate the determinant
            float t16 = (t4*m[8] - t6*m[7] - t8*m[8]+
                        t10*m[7] + t12*m[5] - t14*m[4]);

            // Make sure the determinant is non-zero.
            if (t16 == (float)0.0f)
			{
				return res;
			}
            float t17 = 1/t16;

            res[0] = (m[4]*m[8]-m[5]*m[7])*t17;
            res[1] = -(m[1]*m[8]-m[2]*m[7])*t17;
            res[2] = (m[1]*m[5]-m[2]*m[4])*t17;
            res[3] = -(m[3]*m[8]-m[5]*m[6])*t17;
            res[4] = (m[0]*m[8]-t14)*t17;
            res[5] = -(t6-t10)*t17;
            res[6] = (m[3]*m[7]-m[4]*m[6])*t17;
            res[7] = -(m[0]*m[7]-t12)*t17;
            res[8] = (t4-t8)*t17;
			
			return res;
	}
	
	inline Mat3f transpose (const Mat3f &m)
	{
	Mat3f res(m.v_[0], m.v_[3], m.v_[6],
			 m.v_[1], m.v_[4], m.v_[7],
			 m.v_[2], m.v_[5], m.v_[8]);
	return res;
	}
	

      inline void setIdentity(Mat3f &m) 
      {
	m.v_[0 ] = 1;
	m.v_[1 ] = 0;
	m.v_[2 ] = 0;
	
	m.v_[3 ] = 0;
	m.v_[4 ] = 1;
	m.v_[5 ] = 0;
	
	m.v_[6 ] = 0;
	m.v_[7 ] = 0;
	m.v_[8 ] = 1;

      }

	  
	/**
	 * Sets the matrix to be a skew symmetric matrix based on
	 * the given vector. The skew symmetric matrix is the equivalent
	 * of the vector product. So if a,b are vectors. a x b = A_s b
	 * where A_s is the skew symmetric form of a.
	 */
	inline void setSkewSymmetric(Mat3f &m, const Vec3f vector)
	{
		m[0] = m[4] = m[8] = 0.0f;
		m[1] = -vector.z();
		m[2] = vector.y();
		m[3] = vector.z();
		m[5] = -vector.x();
		m[6] = -vector.y();
		m[7] = vector.x();
	}
	


	/**
	 * Sets the value of the matrix from inertia tensor values.
	 */
	inline void setInertiaTensorCoeffs(Mat3f &m, float ix, float iy, float iz,
		float ixy=0, float ixz=0, float iyz=0)
	{
		m[0] = ix;
		m[1] = m[3] = -ixy;
		m[2] = m[6] = -ixz;
		m[4] = iy;
		m[5] = m[7] = -iyz;
		m[8] = iz;
	}

	inline void setDiagonal(Mat3f &m, float a, float b, float c)
	{
		setInertiaTensorCoeffs(m, a, b, c);
	}
  
		
	} // end namespace standard
	} // end namespace simd
} // end namespace siut